package frsf.cidisi.faia.sensor;

import java.awt.geom.Area;
import java.awt.geom.Ellipse2D;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.awt.geom.Rectangle2D;
import java.util.ArrayList;
import java.util.List;

import frsf.cidisi.faia.sensor.SonarSensor2D;

public class SonarSensor2D extends Sensor2D{

	private double sensorAngle;
    private double sensorApertureAngle;
    private double sensorRange;
    
    private Line2D sensor;
    private double pX;
    private double pY;
    private double distance;
    private int listIndex;
    private SonarSensor2D rebound;
	
    public SonarSensor2D(){
    	super();
    	sensor = new Line2D.Double();
    	distance = 500;
    	listIndex = 1000;
    	rebound = null;
    }
    
    /**
     * @param pX1 the X coordinate position sensor
     * @param pY1 the Y coordinate position sensor
     * @param sensor Beam line sensor
     * @param listaObjetos2D list containing 2D objects (rectangles, cylinders, lines and points)
     */
    public SonarSensor2D (int pX1, int pY1, double angle, double apertureAngle, double range,  List objet2DList) {
    	distance = 500;
    	listIndex = 1000;
    	rebound = null;
    	this.sensorAngle = angle;
        this.sensorRange = range;
        this.sensorApertureAngle = apertureAngle;
        this.objet2D = objet2DList;
        this.pX = pX1;
        this.pY = pY1;
        this.sensorPoint = new Point2D.Double();
        this.sensorPoint.setLocation(pX, pY);
        sensor = new Line2D.Double(pX1, pY1, (pX1+(sensorRange*Math.cos(sensorAngle))),(pY1 +(sensorRange*Math.sin(sensorAngle))));  
        
    }
    
    /**
     * used to calculate the double bounce
     */
    private SonarSensor2D(int pX1, int pY1, Line2D sensor) {
    	this.distance = 500;
    	this.listIndex = 1000;
    	this.rebound = null;
    	this.sensor = sensor;
        this.pX = pX1;
        this.pY = pY1;
        this.sensorPoint = new Point2D.Double();
        this.sensorPoint.setLocation(pX, pY);
        this.objet2D = null;
        sensor = new Line2D.Double();
    	distance = 500;
    	listIndex = 1000;
    	rebound = null;
    }
    
    /**
     * ordeno la linea en un conjunto de puntos para que sea mas sencillo analizarlos
     * @param l una linea2D que será convertida en una lista de rectangulos
     * @return  una lista con Point2D
     */
    public List particionarLinea(){
        List list = new ArrayList();
        double x1, x2, y1, y2;   
        int cut = (int)sensorRange*4;
        
        x1 = sensor.getX1(); x2 = sensor.getX2(); 
        y1 = sensor.getY1(); y2 = sensor.getY2();
        
        // se verifica si la recta del haz del sensor es nula
        if((x1 == x2)&(y1 == y2))
        {return list;}
        
        if(sensorAngle == 4){sensorAngle = getAngle(sensor);}        
        
        double apeX1 = sensorPoint.getX() + (sensorRange * Math.cos(sensorAngle + sensorApertureAngle)) ;
        double apeX2 = sensorPoint.getX() + (sensorRange * Math.cos(sensorAngle - sensorApertureAngle)) ;
        double apeY1 = sensorPoint.getY() + (sensorRange * Math.sin(sensorAngle + sensorApertureAngle)) ;
        double apeY2 = sensorPoint.getY() + (sensorRange * Math.sin(sensorAngle - sensorApertureAngle)) ;
        double incremento = 0.25;
        double difX = Math.abs((Math.abs(Math.abs(apeX1) - Math.abs(apeX2))-incremento)/cut);
        double difY = Math.abs((Math.abs(Math.abs(apeY1) - Math.abs(apeY2))-incremento)/cut);
        double incrementoX = incremento;
        double incrementoY = incremento;

        Point2D.Double centro = new Point2D.Double();
        Point2D.Double diagonalSuperior = new Point2D.Double();
        centro.setLocation(sensorPoint.getX(),sensorPoint.getY());
        
        for(double i = 0; i  < cut; i++)
            {       
                double a = (centro.getX() + (incremento * Math.cos(sensorAngle))); 
                double b = (centro.getY() + (incremento * Math.sin(sensorAngle)));
                centro.setLocation(a, b);
                diagonalSuperior.setLocation(centro.getX() + incrementoX, centro.getY() + incrementoY);
                incrementoX = incrementoX + difX; incrementoY = incrementoY + difY;
                Rectangle2D.Double r = new Rectangle2D.Double();
                r.setFrameFromCenter(centro, diagonalSuperior);
                list.add(r);
            }

        return list;
    }

    /**
     *  si retunr[0] es 500 no se encontro objeto cercano
     * @return int[2] donde return[0] es la distanciaMinima y return[1] es el indice del objeto de menor distancia en la lista
     */
    public double[] distanciaMinima(){
        double[] respuesta = new double[2]; 
        List puntos = particionarLinea();        

        if(this.objet2D.isEmpty()){
            respuesta[0] = this.distance;
            respuesta[1] = this.listIndex;
            System.out.println("objetos vacio \n");
            return respuesta;
        }
        
        if(puntos.isEmpty()){
            respuesta[0] = this.distance;
            respuesta[1] = this.listIndex;
            System.out.println("linea de puntos vacia \n");
            return respuesta;
        }
        
        for(int i = 0; i < this.objet2D.size(); i++){
            Class clase = this.objet2D.get(i).getClass();
            double d = 500;           
            if(clase.isInstance(new java.awt.geom.Point2D.Double())){
                d = distanciaCorte(puntos, (java.awt.geom.Point2D) this.objet2D.get(i)); 
            }
            if(clase.isInstance(new java.awt.geom.Line2D.Double())){
                d = distanciaCorte(puntos, (java.awt.geom.Line2D) this.objet2D.get(i)); 
            }
            if(clase.isInstance(new java.awt.geom.Ellipse2D.Double())){
                d = distanciaCorte(puntos, (java.awt.geom.Ellipse2D) this.objet2D.get(i)); 
            }
            if(d < distance){
                distance = d;
                listIndex = i;
            }
        }
        respuesta[0] = this.distance;
        respuesta[1] = this.listIndex;
        return respuesta;
    }

    /**
     * Analiza el angulo de la linea del sensor y la linea recibida como parametro, devolviendo el angulo de rebote con respecto a la normal de la recta de choque
     * @param l line2D que será analizada para ver el angulo de rebote
     * @return el ángulo de incidencia del sensor sobre la normal de la línea. 
     */
    public double ChoqueLinea(Line2D l){
        
        double pendienteS = getPendiente(sensor);
        double pendienteN = getNormal(l);
        double i = 0;
        try{
     //      System.out.printf("\n La pendiente de S: " + pendienteS);
        // tangente de alfa = (m2 - m1) divido (1 + m2 por m1)
           double b = ((pendienteS - pendienteN)/(1 +(pendienteS * pendienteN)));
           i = (Math.atan(b));
     //      System.out.printf("\n La pendiente de N: " + b + " el angulo es "+ i);
        }
        catch(Exception e){
            i = 0;
        }
        return i;
    }
    
    /**
     * Se analiza el impacto de la linea del sensor con respecto a la esfera.
     * si el valor retornado es 0, es un choque fontal y si es 1 es el peor escenario posible de impacto
     * @param e Ellipse2D que será analizada con la linea del sensor
     * @return un valor Normalizado de la distancia de choque de la linea con la esfera
     */
    public double ChoqueEsfera(Ellipse2D e){
        Point2D centroEsfera= new Point2D.Double();
        centroEsfera.setLocation(e.getCenterX(), e.getCenterY());
        double dist = sensor.ptLineDist(centroEsfera);
        double radio = Math.abs(centroEsfera.getX() - e.getMaxX());

        // Normalizado la respusta, para que se generalize el analisis futuro
        double respuesta = dist / radio;   
        return respuesta;
    }

    
    public boolean rebound(){
        double nx1=0;
        double ny1=0;
        double nangulo = 0;
        double ndistancia=0;
        List nlistaObjetos = null;
        
        if(this.rebound != null){
            return true;
        }
        else{
        	double[] resp = distanciaMinima();
        	double a  = resp[0];
        	double b = resp[1];
        	if(b == 1000){
        		return false;
        	}
        	else{
		        nx1 = sensorPoint.getX()  + (resp[0] * Math.cos(sensorAngle));
		        ny1 = sensorPoint.getY()  + (resp[0] * Math.sin(sensorAngle));
		        ndistancia = (sensorRange - resp[0]);
        
		        Class clase = this.objet2D.get((int)b).getClass();
		        if(clase.isInstance(new java.awt.geom.Line2D.Double())){
		        	Line2D.Double l = (Line2D.Double) this.objet2D.get((int)b);     
		        	double anguloRebote = ChoqueLinea(l);
		        	nangulo = ((Math.PI + sensorAngle)- (anguloRebote*2));
                    System.out.println("\n"+ getAngle(l)+" "+anguloRebote+" "+nangulo+"\n");
		        }
		        
		        if(clase.isInstance(new java.awt.geom.Ellipse2D.Double())){
		            Ellipse2D.Double l = (Ellipse2D.Double) this.objet2D.get((int)b);  
		            double anguloRebote = ChoqueEsfera(l);            
		            nangulo = 0;
		        }
		        
		        List temp = new ArrayList();
		        for(int n = 0; n < this.objet2D.size();n++){
		            temp.add(this.objet2D.get(n));
		        }
		        
		        temp.remove((int)resp[1]);
		        nlistaObjetos = temp;
		        System.out.println(""+nx1+" "+ny1+" "+nangulo+"  "+ndistancia+" ");
		        this.rebound = new SonarSensor2D((int)nx1, (int) ny1, nangulo, this.sensorApertureAngle, ndistancia, nlistaObjetos);
        
		        double[] resp1 = rebound.distanciaMinima(); 
		        double aa  = resp1[0];
		        double bb = resp1[1];
        
		        System.out.println("\n" + aa +"\n");
        	}
        }
        return true;        
    }

    public double getGradeSensorAngle(){
        return  (sensorAngle * 180 )/ Math.PI;
    }
    
	public Line2D getSensor() {
		return sensor;
	}
	public void setSensor(Line2D sensor) {
		this.sensor = sensor;
	}
	public double getSensorAngle() {
		return sensorAngle;
	}
	public void setSensorAngle(double sensorAngle) {
		this.sensorAngle = sensorAngle;
	}
	public double getSensorApertureAngle() {
		return sensorApertureAngle;
	}
	public void setSensorApertureAngle(double sensorApertureAngle) {
		this.sensorApertureAngle = sensorApertureAngle;
	}
	public double getSensorRange() {
		return sensorRange;
	}
	public void setSensorRange(double sensorRange) {
		this.sensorRange = sensorRange;
	}
	public double getpX() {
		return pX;
	}
	public void setpX(double pX) {
		this.pX = pX;
	}
	public double getpY() {
		return pY;
	}
	public void setpY(double pY) {
		this.pY = pY;
	}
	public double getDistance() {
		return distance;
	}
	public void setDistance(double distance) {
		this.distance = distance;
	}
	public int getListIndex() {
		return listIndex;
	}
	public void setListIndex(int listIndex) {
		this.listIndex = listIndex;
	}
	public SonarSensor2D getRebound() {
		return rebound;
	}
	public void setRebound(SonarSensor2D rebound) {
		this.rebound = rebound;
	}

	public static double getAngle(Line2D l){
        double pendiente = getPendiente(l);
        double angle = 0;
        try{
           angle = (Math.atan(pendiente));
        }
        catch(Exception e){
            angle = Math.PI/2;
        }       
        return angle;
	}
	
	  /**
     * Se analiza la pendiente de la linea recibida
     * @return la pendiente de la Linea
     */
    public static double getPendiente(Line2D r){
        double x1 = r.getX1();     
        double x2 = r.getX2(); 
        double y1 = r.getY1(); 
        double y2 = r.getY2();
  
        double pendiente = 0;
        try{
            pendiente = (y2-y1)/(x2-x1);
        }
        catch(Exception e){
                pendiente = 0;
        }   
        return pendiente;
    }
    
    /**
     * Calcula la normal de la pendiente de la linea recibida
     * @param l Line2D la cual se desea obtener la pendiente de la recta normal.
     * @return la pendiente de la recta normal. 
     */
    public static double getNormal(Line2D l) {
        double pendiente;
        try{
         double angulo = getAngle(l);
         double angulo1 = angulo + (Math.PI/2);
       //  System.out.printf("\nFuncion Normal : " + angulo1);            
         pendiente = (Math.tan(angulo1));
        }catch(Exception e)
        {
            pendiente = 999;
        }
        return pendiente;        
    }
    
    /**
     * Transorma un rectangulo en una lista de lineas2D
     * @param rect un rectangulo Rectangle2D que será desglosado
     * @return List de line2D
     */
    public static List RectanguloALinea(Rectangle2D.Double rect){
        List lista = new ArrayList();
        Line2D.Double l1 = new Line2D.Double(rect.getMaxX(),rect.getMaxY(), rect.getMaxX(), rect.getMinY());
        Line2D.Double l2 = new Line2D.Double(rect.getMaxX(),rect.getMaxY(), rect.getMinX(), rect.getMaxY());
        Line2D.Double l3 = new Line2D.Double(rect.getMinX(),rect.getMinY(), rect.getMaxX(), rect.getMinY());
        Line2D.Double l4 = new Line2D.Double(rect.getMinX(),rect.getMinY(), rect.getMinX(), rect.getMaxY());
       
        lista.add(l1);
        lista.add(l2);
        lista.add(l3);
        lista.add(l4);
        
        return lista;
    }
    
    public static Point2D getSensorPoint(double distancia, double angulo){
        return new Point2D.Double((distancia * Math.cos(angulo)),(distancia * Math.sin(angulo)));
    }
    
    private double distanciaCorte(List puntos, Ellipse2D e) {
        double distancia1 = 500;
        if(!puntos.isEmpty()){
            Rectangle2D.Double p = new Rectangle2D.Double();
            for(int i = 0; i < puntos.size(); i++){
               p.setFrame((Rectangle2D) puntos.get(i));
                Area a2 = new Area(e);
                if(a2.intersects(p)){
                   Point2D.Double dd = new Point2D.Double(p.getCenterX(),p.getCenterY());
                   double d = dd.distance(this.sensorPoint);
                   if(d < distancia1)
                       distancia1 = d;
               }
                  
            }                
        } 
        return distancia1;        
    }
    
    private double distanciaCorte(List puntos, Line2D e) {
    	double distancia1 = 500;
        // trazo 5 rectas con diferentes angulos para facilitar los calculos.
/* 
        Line2D.Double l1 = new Line2D.Double(pX, pY, alcanceSensor + Math.cos(anguloSensor + anguloApertura),alcanceSensor + Math.sin(anguloSensor + anguloApertura));
        Line2D.Double l2 = new Line2D.Double(pX, pY, alcanceSensor + Math.cos(anguloSensor - anguloApertura),alcanceSensor + Math.sin(anguloSensor - anguloApertura));
        Line2D.Double l3 = new Line2D.Double(pX, pY, alcanceSensor + Math.cos(anguloSensor + anguloApertura/2),alcanceSensor + Math.sin(anguloSensor + anguloApertura/2));
        Line2D.Double l4 = new Line2D.Double(pX, pY, alcanceSensor + Math.cos(anguloSensor - anguloApertura/2),alcanceSensor + Math.sin(anguloSensor - anguloApertura/2));
        Line2D.Double l5 = new Line2D.Double(pX, pY, alcanceSensor + Math.cos(anguloSensor),alcanceSensor + Math.sin(anguloSensor));
         if(Line2D.linesIntersect(l1.getX1(), l1.getY1(), l1.getX2(), l1.getY2(),
                e.getX1(),e.getY1(),e.getX2(),e.getY2())
                ||Line2D.linesIntersect(l2.getX1(), l2.getY1(), l2.getX2(), l2.getY2(),
                e.getX1(),e.getY1(),e.getX2(),e.getY2())
                ||Line2D.linesIntersect(l3.getX1(), l3.getY1(), l3.getX2(), l3.getY2(),
                e.getX1(),e.getY1(),e.getX2(),e.getY2())
                ||Line2D.linesIntersect(l4.getX1(), l4.getY1(), l4.getX2(), l4.getY2(),
                e.getX1(),e.getY1(),e.getX2(),e.getY2())
                ||Line2D.linesIntersect(l5.getX1(), l5.getY1(), l5.getX2(), l5.getY2(),
                e.getX1(),e.getY1(),e.getX2(),e.getY2())){
    
       */      
    	if(!puntos.isEmpty()){
    		Rectangle2D.Double p = new Rectangle2D.Double();
    		for(int i = 0; i < puntos.size(); i++){
    			p.setFrame((Rectangle2D) puntos.get(i));
    			if(p.intersectsLine(e)){
    				Point2D.Double dd = new Point2D.Double(p.getCenterX(),p.getCenterY());
    				double d = dd.distance(this.sensorPoint);
    				if(d < distancia1) distancia1 = d;
    			}          
    		}                     
    	}
    	
    	return distancia1;        
    }
    
    
    /**
     * Debe retornar un nùmero menor que 500 para que exista intersección
     * @param puntos la List de Rectangle2D que serán comparados con la figura
     * @param e Point2D a comparar
     * @return la distancias desde el sensor hasta el objeto, retorna 500 si no se intersectan
     */
    private double distanciaCorte(List puntos, Point2D e) {
        double distancia1 = 500;
        if(!puntos.isEmpty()){
	        Rectangle2D.Double p = new Rectangle2D.Double();
	        for(int i = 0; i < puntos.size(); i++){
	           p.setFrame((Rectangle2D) puntos.get(i));
	           if(p.contains(e)){
	               Point2D.Double dd = new Point2D.Double(p.getCenterX(),p.getCenterY());
	               double d = dd.distance(this.sensorPoint);
	               if(d < distancia1) distancia1 = d;
	           }
	        } 
        }
        return distancia1;        
    }
    
    /**
     * Falta realizar si el rebote es doble... es decir, si la linea que rebota, llega luego de rebotar nuevamente con otro obstaculo
     */
    
}
